package main

import (
	"fmt"
	"log"
	"sync"
	"time"

	modbus "gitee.com/barryzxy/gomodbus"
)

// FIFOQueue 自定义FIFO队列实现
type FIFOQueue struct {
	mu       sync.RWMutex
	data     []uint16 // FIFO数据
	maxSize  int      // 最大队列大小
	readPtr  int      // 读指针
	writePtr int      // 写指针
	count    int      // 当前元素数量
}

// NewFIFOQueue 创建新的FIFO队列
func NewFIFOQueue(maxSize int) *FIFOQueue {
	return &FIFOQueue{
		data:    make([]uint16, maxSize),
		maxSize: maxSize,
	}
}

// Push 向队列中添加数据
func (f *FIFOQueue) Push(value uint16) bool {
	f.mu.Lock()
	defer f.mu.Unlock()

	if f.count >= f.maxSize {
		return false // 队列已满
	}

	f.data[f.writePtr] = value
	f.writePtr = (f.writePtr + 1) % f.maxSize
	f.count++
	return true
}

// Pop 从队列中读取数据
func (f *FIFOQueue) Pop() (uint16, bool) {
	f.mu.Lock()
	defer f.mu.Unlock()

	if f.count == 0 {
		return 0, false // 队列为空
	}

	value := f.data[f.readPtr]
	f.readPtr = (f.readPtr + 1) % f.maxSize
	f.count--
	return value, true
}

// ReadAll 读取所有数据（不清空队列）
func (f *FIFOQueue) ReadAll() []uint16 {
	f.mu.RLock()
	defer f.mu.RUnlock()

	if f.count == 0 {
		return nil
	}

	result := make([]uint16, f.count)
	for i := 0; i < f.count; i++ {
		pos := (f.readPtr + i) % f.maxSize
		result[i] = f.data[pos]
	}
	return result
}

// Count 获取当前队列元素数量
func (f *FIFOQueue) Count() int {
	f.mu.RLock()
	defer f.mu.RUnlock()
	return f.count
}

// Clear 清空队列
func (f *FIFOQueue) Clear() {
	f.mu.Lock()
	defer f.mu.Unlock()
	f.readPtr = 0
	f.writePtr = 0
	f.count = 0
}

// CustomFIFONodeRegister 自定义支持FIFO的节点寄存器
type CustomFIFONodeRegister struct {
	*modbus.NodeRegister
	fifoQueues map[uint16]*FIFOQueue // 地址到FIFO队列的映射
	mu         sync.RWMutex
}

// NewCustomFIFONodeRegister 创建自定义FIFO节点寄存器
func NewCustomFIFONodeRegister(slaveID byte,
	coilsAddrStart, coilsQuantity,
	discreteAddrStart, discreteQuantity,
	inputAddrStart, inputQuantity,
	holdingAddrStart, holdingQuantity uint16) *CustomFIFONodeRegister {

	return &CustomFIFONodeRegister{
		NodeRegister: modbus.NewNodeRegister(slaveID,
			coilsAddrStart, coilsQuantity,
			discreteAddrStart, discreteQuantity,
			inputAddrStart, inputQuantity,
			holdingAddrStart, holdingQuantity),
		fifoQueues: make(map[uint16]*FIFOQueue),
	}
}

// CreateFIFOQueue 在指定地址创建FIFO队列
func (c *CustomFIFONodeRegister) CreateFIFOQueue(address uint16, maxSize int) {
	c.mu.Lock()
	defer c.mu.Unlock()
	c.fifoQueues[address] = NewFIFOQueue(maxSize)
}

// PushToFIFO 向指定地址的FIFO队列添加数据
func (c *CustomFIFONodeRegister) PushToFIFO(address uint16, value uint16) bool {
	c.mu.RLock()
	fifo, exists := c.fifoQueues[address]
	c.mu.RUnlock()

	if !exists {
		return false
	}
	return fifo.Push(value)
}

// ReadFIFOQueue 读取FIFO队列数据（实现Modbus 0x18功能码）
func (c *CustomFIFONodeRegister) ReadFIFOQueue(address uint16) ([]byte, error) {
	c.mu.RLock()
	fifo, exists := c.fifoQueues[address]
	c.mu.RUnlock()

	if !exists {
		return nil, &modbus.ExceptionError{ExceptionCode: modbus.ExceptionCodeIllegalDataAddress}
	}

	// 读取FIFO数据
	fifoData := fifo.ReadAll()
	fifoCount := uint16(len(fifoData))

	// 构建Modbus FIFO响应格式
	// 字节计数(2) + FIFO计数(2) + FIFO数据(n*2)
	byteCount := uint16(4 + len(fifoData)*2)

	result := make([]byte, 0, byteCount)
	// 字节计数
	result = append(result, byte(byteCount>>8), byte(byteCount))
	// FIFO计数
	result = append(result, byte(fifoCount>>8), byte(fifoCount))
	// FIFO数据
	for _, value := range fifoData {
		result = append(result, byte(value>>8), byte(value))
	}

	return result, nil
}

// CustomFIFOServer 自定义FIFO服务器
type CustomFIFOServer struct {
	*modbus.TCPServer
	customNodes map[byte]*CustomFIFONodeRegister
	mu          sync.RWMutex
}

// NewCustomFIFOServer 创建自定义FIFO服务器
func NewCustomFIFOServer() *CustomFIFOServer {
	server := &CustomFIFOServer{
		TCPServer:   modbus.NewTCPServer(),
		customNodes: make(map[byte]*CustomFIFONodeRegister),
	}

	// 注册自定义FIFO功能码处理器
	server.RegisterFunctionHandler(modbus.FuncCodeReadFIFOQueue, server.handleReadFIFOQueue)

	return server
}

// AddCustomFIFONode 添加自定义FIFO节点
func (s *CustomFIFOServer) AddCustomFIFONode(node *CustomFIFONodeRegister) {
	s.mu.Lock()
	defer s.mu.Unlock()

	slaveID := node.SlaveID()
	s.customNodes[slaveID] = node
	s.AddNodes(node.NodeRegister)
}

// handleReadFIFOQueue 处理FIFO队列读取请求
func (s *CustomFIFOServer) handleReadFIFOQueue(reg *modbus.NodeRegister, data []byte) ([]byte, error) {
	if len(data) != 2 {
		return nil, &modbus.ExceptionError{ExceptionCode: modbus.ExceptionCodeIllegalDataValue}
	}

	address := uint16(data[0])<<8 | uint16(data[1])
	slaveID := reg.SlaveID()

	s.mu.RLock()
	customNode, exists := s.customNodes[slaveID]
	s.mu.RUnlock()

	if !exists {
		return nil, &modbus.ExceptionError{ExceptionCode: modbus.ExceptionCodeIllegalDataAddress}
	}

	return customNode.ReadFIFOQueue(address)
}

func main() {
	fmt.Println("🚀 启动自定义FIFO队列Modbus服务器示例...")

	// 创建自定义FIFO服务器
	server := NewCustomFIFOServer()
	server.LogMode(true)

	// 创建自定义FIFO节点
	fifoNode := NewCustomFIFONodeRegister(
		1,     // 从设备ID
		0, 10, // 线圈范围
		0, 10, // 离散输入范围
		0, 10, // 输入寄存器范围
		0, 100, // 保持寄存器范围
	)

	// 在地址100-109创建FIFO队列
	for addr := uint16(100); addr < 110; addr++ {
		fifoNode.CreateFIFOQueue(addr, 20) // 每个队列最大20个元素
		fmt.Printf("📊 创建FIFO队列: 地址 %d, 最大容量 20\n", addr)
	}

	// 添加节点到服务器
	server.AddCustomFIFONode(fifoNode)

	// 启动数据生成器（模拟FIFO数据产生）
	go func() {
		ticker := time.NewTicker(500 * time.Millisecond)
		defer ticker.Stop()

		counter := uint16(1000)
		for range ticker.C {
			for addr := uint16(100); addr < 110; addr++ {
				// 向每个FIFO队列添加数据
				if fifoNode.PushToFIFO(addr, counter) {
					fmt.Printf("📥 FIFO[%d] <- %d\n", addr, counter)
				} else {
					fmt.Printf("⚠️  FIFO[%d] 队列已满\n", addr)
				}
				counter++
			}
		}
	}()

	// 启动客户端测试
	go func() {
		time.Sleep(2 * time.Second) // 等待服务器启动
		testFIFOClient()
	}()

	fmt.Println("🎯 服务器启动在端口 :502")
	fmt.Println("💡 使用以下命令测试FIFO功能：")
	fmt.Println("   - 功能码 0x18 (24)")
	fmt.Println("   - 地址范围: 100-109")
	fmt.Println("   - 每个FIFO队列容量: 20")

	// 启动服务器
	if err := server.ListenAndServe(":502"); err != nil {
		log.Fatalf("服务器启动失败: %v", err)
	}
}

// testFIFOClient 测试FIFO客户端
func testFIFOClient() {
	fmt.Println("\n🧪 开始FIFO客户端测试...")

	// 创建客户端
	provider := modbus.NewTCPClientProvider("localhost:502", modbus.WithEnableLogger())
	client := modbus.NewClient(provider)

	if err := client.Connect(); err != nil {
		fmt.Printf("❌ 连接失败: %v\n", err)
		return
	}
	defer client.Close()

	// 等待一些数据积累
	time.Sleep(3 * time.Second)

	// 测试FIFO队列读取
	for addr := uint16(100); addr < 105; addr++ {
		fmt.Printf("\n📖 读取FIFO队列 地址=%d:\n", addr)

		data, err := client.ReadFIFOQueue(1, addr)
		if err != nil {
			fmt.Printf("❌ 读取失败: %v\n", err)
			continue
		}

		if len(data) < 4 {
			fmt.Printf("❌ 响应数据太短\n")
			continue
		}

		// 解析FIFO响应
		byteCount := uint16(data[0])<<8 | uint16(data[1])
		fifoCount := uint16(data[2])<<8 | uint16(data[3])

		fmt.Printf("  📊 字节计数: %d\n", byteCount)
		fmt.Printf("  📊 FIFO计数: %d\n", fifoCount)

		if len(data) >= 4+int(fifoCount)*2 {
			fmt.Printf("  📋 FIFO数据: ")
			for i := 0; i < int(fifoCount); i++ {
				pos := 4 + i*2
				value := uint16(data[pos])<<8 | uint16(data[pos+1])
				fmt.Printf("%d ", value)
			}
			fmt.Println()
		}
	}

	fmt.Println("\n✅ FIFO客户端测试完成!")
}
